/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package nx2.behaviors;

import nx2.utils.*;
import javaclient2.Position2DInterface;
/**
 *
 * @author alejo
 */
public class NxGoToPoint extends NxBehavior{

    /**
     * Maximum distance to the goal. Beyond this distance the vectors magnitude
     * is the maximum (1).
     */
    private double maxDist = 4.0f;

    /**
     * Minimal distance to the goal.
     */
    private double minDist = 0.1f;

    /**
     * The point where the robot should go.
     */
    private Vector2D goalPoint;

    /**
     * The position interface from where to calculate the distance to goalPoint
     */
    private Position2DInterface pos;

    /**
     * A behavior that generates the goalpoint.
     */
    private NxBehavior goalGenerator;

    /**
     * Indicates if a behavior is used to generate the goalpoint.
     */
    private boolean isGoalGenerator = false;

    private float minVel = 0;

    public NxGoToPoint(String name,Position2DInterface pos, Vector2D goalPoint, double maxDist, double minDist)
    {
        super(name);
        this.pos = pos;
        this.goalPoint = goalPoint;

        this.setParamsDist(minDist, maxDist);
    }

    public NxGoToPoint(String name,Position2DInterface pos, NxBehavior goalGen, double maxDist, double minDist) {

        super(name);

        this.pos = pos;
        goalGenerator = goalGen;

        isGoalGenerator = true;
        this.setParamsDist(minDist, maxDist);
        

    }

    @Override
    public Vector2D value()
    {

        //TODO: por definir como variables de la clase
        float Kp = 3;
        float minAngle = 0.5f;

        
        //while(!pos.isDataReady());
        if(isGoalGenerator)
        {
            goalPoint = (Vector2D)goalGenerator.value();
        }

        //position of the robot.
        Vector2D myPos = new Vector2D(pos.getData().getPos());

        //heading of the robot
        float myHeading = pos.getYaw()*(float)Math.PI/180;

        //vector directed to the goalPoint
        Vector2D vectToPoint = goalPoint.sub(myPos);

        System.out.print("myPos:");
        myPos.print(Vector2D.MODE_XY);
        System.out.print("vectToPoint:");
        vectToPoint.print(Vector2D.MODE_XY);
        System.out.println("vectToPoint angle: "+ vectToPoint.getAngle() + " myHeading: "+myHeading);


        //first calculate the angular velocity
        float rotVel = vectToPoint.getAngle() - myHeading;

        //scale vector to obtain a normalized velocity between (-1,1)
        rotVel /= Math.PI;

        rotVel *= Kp;

        if(rotVel>1)
            rotVel = 1;
        if(rotVel<-1)
            rotVel = -1;
       

        //Then calculate translational velocity
        float transVel = 0;
        float distToPoint = vectToPoint.getMag();
        if(distToPoint >= this.maxDist)
            transVel = 1;
        else if (distToPoint < this.minDist)
            transVel = 0;
        else
        {
            transVel = (distToPoint - (float)this.minDist)/((float)this.maxDist- (float)this.minDist);
        }

        if(Math.abs(rotVel)>minAngle)
            transVel = 0;
        else
            transVel *= (1-Math.abs(rotVel));

        System.out.println("rotVel: "+rotVel);
        System.out.println("transVel: "+transVel);

        return new Vector2D(transVel,rotVel,Vector2D.MODE_XY);
    }


    /**
     * Sets maxDist.
     * @param maxDist New value for maxDist.
     */
    public void setMaxDist(float maxDist)
    {
        this.maxDist = maxDist;
    }
    /**
     * Sets minDist.
     * @param minDist New value for minDist.
     */
    public void setMinDist(float minDist)
    {
        this.minDist = minDist;
    }

    public void changeGoalPoint(Vector2D goalPoint)
    {
        this.goalPoint = goalPoint;
    }

    /**
     * Set function that combines minDist and maxDist.
     * @param minDist New value for minDist.
     * @param maxDist New value for maxDist.
     */
    public void setParamsDist(double minDist, double maxDist)
    {
        this.maxDist = maxDist;
        this.minDist = minDist;
    }

    /**
     * Sets the minimum velocity of this behavior.
     * @param minVel The new value for minimum velocity.
     */
    public void setMinVel(float minVel)
    {
        this.minVel = minVel;
    }

    /**
     * Gets the minimum velocity configured to this behavior.
     * @return The minimum velocity configured.
     */
    public float getMinVel()
    {
        return this.minVel;
    }

}
